package trafficmanager

import (
	"bytes"
	"context"
	"encoding/gob"
	"encoding/json"
	"errors"
	"math"
	"strings"
	"sync"
	"time"

	"dev.ixmxm.top/rms/boot"
	"dev.ixmxm.top/rms/codes"
	"dev.ixmxm.top/rms/managers/devicemanager"
	"dev.ixmxm.top/rms/managers/locationmanager"
	"dev.ixmxm.top/rms/managers/robotmanager"
	"dev.ixmxm.top/rms/managers/taskmanager"
	"dev.ixmxm.top/rms/model"
	"dev.ixmxm.top/rms/proto"
	"dev.ixmxm.top/rms/utils/function"
	"dev.ixmxm.top/rms/utils/log"
	"dev.ixmxm.top/rms/utils/tools"
	"go.mongodb.org/mongo-driver/bson"
	"go.mongodb.org/mongo-driver/mongo"
	"go.mongodb.org/mongo-driver/mongo/options"
)

func NewTrafficManager(db *boot.MongoDB, devManager *devicemanager.DeviceManager, taskManager *taskmanager.TaskManager, locationManager1 *locationmanager.LocationManager) *TrafficManager {
	tm := &TrafficManager{db: db, mapNameMapping: make(map[string]mapInfo, 0), crossPlaner: newMultiMapPlaner(db), devManager: devManager, taskManager: taskManager, locationManager: locationManager1, planers: make(map[string]*Planer, 0)}
	tm.Init()
	return tm
}

type TrafficManager struct {
	db                    *boot.MongoDB
	devManager            *devicemanager.DeviceManager
	crossPlaner           *MultiMapPlaner
	mapLocker             sync.RWMutex
	mapRelationLocker     sync.RWMutex
	areaLocker            sync.RWMutex
	standbySiteLocker     sync.Mutex
	saveStandBySiteLocker sync.Mutex
	mapNameMapping        map[string]mapInfo
	planers               map[string]*Planer
	SystemConfig          *model.SystemConfig //系统配置(缓存),不可直接赋值保存，调用SaveSystemConfigToCache保存
	taskManager           *taskmanager.TaskManager
	locationManager       *locationmanager.LocationManager //库位管理
}

//InitDijkstraMaps 初始化代价地图
func (tm *TrafficManager) InitDijkstraMaps(robotTypes ...*model.RobotType) {
	for _, p := range tm.planers {
		for _, t := range robotTypes {
			p.initDijkstraMap(*t)
		}
	}
}

//获得所有的厂区切换点的状态
func (tm *TrafficManager) GetAllMapRelationPointStatus() []model.MapRelationPoint {
	newMapRelations, _ := tm.GetAllMapRelationFromDB(context.TODO())
	if newMapRelations != nil {
		var findMapRelationUsedArr []model.MapRelationPoint
		var findMapRelationUsedMap map[string]model.MapRelationPoint
		findMapRelationUsedMap = make(map[string]model.MapRelationPoint, 0)

		for _, mapRelation := range newMapRelations {
			//1.先剔除掉所有不是厂区切换点的类型
			if mapRelation.Type != model.FactoryRealtion {
				continue
			}
			//2.先将包含AGV状态的厂区切换点找出来
			if mapRelation.AgvId != "" {
				//判断是否包含了该key
				if _, ok := findMapRelationUsedMap[mapRelation.FromMap+"+"+mapRelation.StartId]; !ok {
					var mapRelationPointStart model.MapRelationPoint
					mapRelationPointStart.MapName = mapRelation.FromMap
					mapRelationPointStart.SiteId = mapRelation.StartId
					mapRelationPointStart.AgvId = mapRelation.AgvId
					mapRelationPointStart.Positive = mapRelation.PositiveWay
					mapRelationPointStart.FromMap = mapRelation.FromMap
					mapRelationPointStart.StartId = mapRelation.StartId
					mapRelationPointStart.ToMap = mapRelation.ToMap
					mapRelationPointStart.GoalId = mapRelation.GoalId
					findMapRelationUsedMap[mapRelation.FromMap+"+"+mapRelation.StartId] = mapRelationPointStart
					//初始化实体
					findMapRelationUsedArr = append(findMapRelationUsedArr, mapRelationPointStart)
				}
				if _, ok := findMapRelationUsedMap[mapRelation.ToMap+"+"+mapRelation.GoalId]; !ok {
					var mapRelationPointGoal model.MapRelationPoint
					mapRelationPointGoal.MapName = mapRelation.ToMap
					mapRelationPointGoal.SiteId = mapRelation.GoalId
					mapRelationPointGoal.AgvId = mapRelation.AgvId
					mapRelationPointGoal.Positive = !mapRelation.PositiveWay
					mapRelationPointGoal.FromMap = mapRelation.FromMap
					mapRelationPointGoal.StartId = mapRelation.StartId
					mapRelationPointGoal.ToMap = mapRelation.ToMap
					mapRelationPointGoal.GoalId = mapRelation.GoalId
					findMapRelationUsedMap[mapRelation.ToMap+"+"+mapRelation.GoalId] = mapRelationPointGoal
					findMapRelationUsedArr = append(findMapRelationUsedArr, mapRelationPointGoal)
				}

			}
		}
		//过滤结束后
		return findMapRelationUsedArr
	}
	return nil
}

//释放厂区占用
func (tm *TrafficManager) ReleaseMapRelationPointStatus(ctx context.Context, fromMap string, fromSiteId string, toMap string, toSiteId string) codes.Code {
	tm.mapRelationLocker.Lock()
	defer tm.mapRelationLocker.Unlock()
	if tm.crossPlaner.mapRelations == nil {
		return codes.ErrFactoryRelationMapsIsNull
	}

	for _, mapRelation := range tm.crossPlaner.mapRelations {
		if mapRelation.Type != model.FactoryRealtion {
			continue
		}
		//判断
		if mapRelation.FromMap == fromMap && mapRelation.StartId == fromSiteId && mapRelation.ToMap == toMap && mapRelation.GoalId == toSiteId {
			err := tm.ReleaseFactoryAgvIdToDB(ctx, fromMap, fromSiteId, toMap, toSiteId)
			if err != nil {
				return err
			}
			mapRelation.AgvId = ""
			return nil
		}

	}
	//遍历所有厂区切换，找出满足条件的厂区切换数据
	return nil
}

//释放充电桩-数据库
func (tm *TrafficManager) ReleaseFactoryAgvIdToDB(ctx context.Context, fromMap, fromSiteId, toMap, toSiteId string) codes.Code {

	dbRes := tm.db.Collection(ctx, model.MapRelation{}.TableName()).FindOneAndUpdate(ctx, bson.M{"from_map": fromMap, "start_id": fromSiteId, "to_map": toMap, "goal_id": toSiteId}, bson.M{"$set": bson.M{"agv_id": ""}})
	if dbRes.Err() != nil {
		return codes.ErrDatabase.Wrap(dbRes.Err())
	}

	return nil
}
func DeepCopy(src, dst interface{}) error {
	var buf bytes.Buffer
	enc := gob.NewEncoder(&buf)
	dec := gob.NewDecoder(&buf)

	if err := enc.Encode(src); err != nil {
		return err
	}

	return dec.Decode(dst)
}

//SaveMapToDB 存储地图到db
func (tm *TrafficManager) SaveMapToDB(ctx context.Context, m *model.Map) codes.Code {
	tm.mapLocker.Lock()
	defer tm.mapLocker.Unlock()
	//将一个对象拆成3个对象
	ctxTx := boot.Begin(ctx)
	for i := 0; i < 3; i++ {
		//拷贝
		m1 := &model.Map{}
		err1 := DeepCopy(m, m1)
		if err1 != nil {
			ctxTx.Rollback()
			return codes.ErrDatabase.Wrap(err1)
		}
		if i == int(model.MapSaveTypeMapDataNodes) {
			m1.MapData.Paths = make(map[string]model.Path, 0)
			m1.MapData.Areas = make(map[string]model.Area, 0)
			m1.MapData.Obstacle = make(map[string]model.Obstacle, 0)
			m1.MapData.MarkArea = make(map[string]model.MarkArea, 0)
			//
			// m1.MapDataJsonString = ""
			// m1.RawBase64 = ""
			m1.Type = model.MapSaveTypeMapDataNodes

		} else if i == int(model.MapSaveTypeMapDataPath) {
			m1.MapData.NodesMap = make(map[string]model.Node, 0)
			m1.MapData.Areas = make(map[string]model.Area, 0)
			m1.MapData.Obstacle = make(map[string]model.Obstacle, 0)
			m1.MapData.MarkArea = make(map[string]model.MarkArea, 0)
			//
			m1.MapDataJsonString = ""
			m1.RawBase64 = ""
			m1.Type = model.MapSaveTypeMapDataPath

		} else if i == int(model.MapSaveTypeMapDataArea) {
			m1.MapData.NodesMap = make(map[string]model.Node, 0)
			m1.MapData.Paths = make(map[string]model.Path, 0)
			//清空
			m1.MapDataJsonString = ""
			m1.RawBase64 = ""
			m1.Type = model.MapSaveTypeMapDataArea
		}

		dbRes := tm.db.Collection(ctx, m.TableName()).FindOneAndReplace(ctx, bson.M{"map_name": m.MapName, "type": i}, m1, &options.FindOneAndReplaceOptions{Upsert: function.BoolPtr(true)})
		if dbRes.Err() != nil && !errors.Is(dbRes.Err(), mongo.ErrNoDocuments) {
			log.ErrorfWithContext(ctx, "find map from db error:%s", dbRes.Err().Error())
			ctxTx.Rollback()
			return codes.ErrDatabase.Wrap(dbRes.Err())
		}
	}
	ctxTx.Commit()

	if errRefresh := tm.refreshMapStandbyPoints(ctx, m); errRefresh != nil {
		log.ErrorfWithContext(ctx, "refresh standby point error:%s", errRefresh.UnWrapError())
		return errRefresh
	}
	//删除锁定区域
	lockAreas, err := tm.GetMapLockAreaFromDB(ctx, m.MapName)
	if err != nil {
		log.ErrorfWithContext(ctx, "保存地图，获取锁定区域发生错误 error:%s", err.UnWrapError())
		return err
	}
	for _, lockArea := range lockAreas {
		isAny := false
		for _, area := range m.MapData.Areas {
			if area.Type == model.AreaTypeInteractive && area.Id == lockArea.AreaId {
				isAny = true
				break
			}
		}
		if !isAny {
			err = tm.DeleteLockAreaFromDB(ctx, m.MapName, lockArea.AreaId)
			if err != nil {
				log.ErrorfWithContext(ctx, "保存地图,删除锁定区域(%d)发生错误 error:%s", lockArea.AreaId, err.UnWrapError())
				return err
			}
		}
	}
	//更新地图映射
	tm.mapNameMapping[m.MapName] = mapInfo{m.ProjectId, m.TrafficName}
	//更新planer
	err = tm.devManager.Load(m.MapName, m.MapData.NodesMap)
	if err != nil {
		return err
	}

	// tm.planers[m.MapName] = newPlaner(m, tm.db, tm.devManager, tm.robotmanager, tm.taskManager)
	tm.ReInit(m)
	return nil
}

//SaveMapToDB 存储地图到db
func (tm *TrafficManager) SaveOverviewMapToDB(ctx context.Context, m *model.MapOverview) codes.Code {
	dbRes := tm.db.Collection(ctx, m.TableName()).FindOneAndReplace(ctx, bson.M{"name": m.Name}, m, &options.FindOneAndReplaceOptions{Upsert: function.BoolPtr(true)})
	if dbRes.Err() != nil && !errors.Is(dbRes.Err(), mongo.ErrNoDocuments) {
		log.ErrorfWithContext(ctx, "find map from db error:%s", dbRes.Err().Error())
		return codes.ErrDatabase.Wrap(dbRes.Err())
	}
	return nil
}

//GetAllMapFromDB 从数据库获得所有地图
func (tm *TrafficManager) GetAllMapFromDB(ctx context.Context) ([]*model.Map, codes.Code) {
	maps := make([]*model.Map, 0)
	res, err := tm.db.Collection(ctx, model.Map{}.TableName()).Find(ctx, bson.D{})
	if err != nil {
		log.ErrorfWithContext(ctx, "get all map from db error:%s", err.Error())
		return nil, codes.ErrDatabase.Wrap(err)
	}
	if errBind := res.All(ctx, &maps); errBind != nil {
		log.ErrorfWithContext(ctx, "bind all maps error:%s", errBind.Error())
		return nil, codes.ErrDatabase.Wrap(errBind)
	}
	if len(maps) <= 0 {
		log.Errorf("获取地图数据库失败")
		return nil, nil
	}
	//将数据归整
	updateData := func(dict map[string]*model.Map, map12 *model.Map) {
		if map12.Type == model.MapSaveTypeMapDataNodes {
			//
			dict[map12.MapName].MapData.MapInfo = map12.MapData.MapInfo
			dict[map12.MapName].MapData.NodesMap = map12.MapData.NodesMap
			//
			dict[map12.MapName].MapDataJsonString = map12.MapDataJsonString
			dict[map12.MapName].RawBase64 = map12.RawBase64
		} else if map12.Type == model.MapSaveTypeMapDataPath {
			//
			dict[map12.MapName].MapData.Paths = map12.MapData.Paths
			//
		} else if map12.Type == model.MapSaveTypeMapDataArea {
			//
			dict[map12.MapName].MapData.Areas = map12.MapData.Areas
			dict[map12.MapName].MapData.Obstacle = map12.MapData.Obstacle
			dict[map12.MapName].MapData.MarkArea = map12.MapData.MarkArea
			//
		}
	}
	mapsDict := make(map[string]*model.Map, 0)
	for _, map12 := range maps {
		if _, find := mapsDict[map12.MapName]; find {
			updateData(mapsDict, map12)
			continue
		}
		mapsDict[map12.MapName] = map12
		updateData(mapsDict, map12)
	}
	maps1 := make([]*model.Map, 0)
	for _, value := range mapsDict {
		maps1 = append(maps1, value)
	}

	return maps1, nil
}

//GetAllOverviewMapFromDB 从数据获得
func (tm *TrafficManager) GetAllOverviewMapFromDB(ctx context.Context) ([]*model.MapOverview, codes.Code) {
	overviewMaps := make([]*model.MapOverview, 0)
	res, err := tm.db.Collection(ctx, model.MapOverview{}.TableName()).Find(ctx, bson.D{})
	if err != nil {
		log.ErrorfWithContext(ctx, "get all overview map from db error:%s", err.Error())
		return nil, codes.ErrDatabase.Wrap(err)
	}
	if errBind := res.All(ctx, &overviewMaps); errBind != nil {
		log.ErrorfWithContext(ctx, "bind all overview maps error:%s", errBind.Error())
		return nil, codes.ErrDatabase.Wrap(errBind)
	}
	return overviewMaps, nil
}

//GetMapFromDB 从数据库获得地图
func (tm *TrafficManager) GetMapFromDB(ctx context.Context, mapName string) (*model.Map, codes.Code) {
	maps := make([]*model.Map, 0)
	res, err := tm.db.Collection(ctx, model.Map{}.TableName()).Find(ctx, bson.M{"map_name": mapName})
	if err != nil {
		log.ErrorfWithContext(ctx, "get  map from db error:%s", err.Error())
		return nil, codes.ErrDatabase.Wrap(err)
	}
	if errBind := res.All(ctx, &maps); errBind != nil {
		log.ErrorfWithContext(ctx, "bind all maps error:%s", errBind.Error())
		return nil, codes.ErrDatabase.Wrap(errBind)
	}
	//将数据归整
	if len(maps) < 0 {
		log.Info(ctx, " maps len <0  error")
		return nil, nil
	}
	map1 := maps[0]
	if len(maps) > 1 {
		for _, map12 := range maps {
			if map12.Type == model.MapSaveTypeMapDataNodes {
				//
				map1.MapData.MapInfo = map12.MapData.MapInfo
				map1.MapData.NodesMap = map12.MapData.NodesMap
				//
				map1.MapDataJsonString = map12.MapDataJsonString
				map1.RawBase64 = map12.RawBase64
			} else if map12.Type == model.MapSaveTypeMapDataPath {
				//
				map1.MapData.Paths = map12.MapData.Paths
				//
			} else if map12.Type == model.MapSaveTypeMapDataArea {
				map1.MapData.Areas = map12.MapData.Areas
				map1.MapData.Obstacle = map12.MapData.Obstacle
				map1.MapData.MarkArea = map12.MapData.MarkArea
			}
		}
	}
	// map1.MapData.ParseMapNodeAssociatedNodeBlocks()

	//将mapsDict转换成maps
	return map1, nil
}

//GetMapByNameFromDB 从数据库获得指定厂区和楼层的地图
func (tm *TrafficManager) GetMapByNameFromDB(ctx context.Context, mapName string) (*model.Map, codes.Code) {
	maps := make([]*model.Map, 0)
	res, err := tm.db.Collection(ctx, model.Map{}.TableName()).Find(ctx, bson.M{"map_name": mapName})
	if err != nil {
		log.ErrorfWithContext(ctx, "get  map from db error:%s", err.Error())
		return nil, codes.ErrDatabase.Wrap(err)
	}
	if errBind := res.All(ctx, &maps); errBind != nil {
		log.ErrorfWithContext(ctx, "bind all maps error:%s", errBind.Error())
		return nil, codes.ErrDatabase.Wrap(errBind)
	}
	//将数据归整
	if len(maps) < 0 {
		log.Info(ctx, "maps len <0  error")
		return nil, codes.ErrMapNotFound
	}
	map1 := maps[0]
	if len(maps) > 1 {
		for _, map12 := range maps {
			if map12.Type == model.MapSaveTypeMapDataNodes {
				//
				map1.MapData.MapInfo = map12.MapData.MapInfo
				map1.MapData.NodesMap = map12.MapData.NodesMap
				//
				map1.MapDataJsonString = map12.MapDataJsonString
				map1.RawBase64 = map12.RawBase64
			} else if map12.Type == model.MapSaveTypeMapDataPath {
				//
				map1.MapData.Paths = map12.MapData.Paths
				//
			} else if map12.Type == model.MapSaveTypeMapDataArea {
				map1.MapData.Areas = map12.MapData.Areas
				map1.MapData.Obstacle = map12.MapData.Obstacle
				map1.MapData.MarkArea = map12.MapData.MarkArea
			}

		}
	}
	//将mapsDict转换成maps
	return map1, nil
}

//DeleteMapFromDB 从db删除指定地图
func (tm *TrafficManager) DeleteMapFromDB(ctx context.Context, mapName string) codes.Code {
	_, err := tm.db.Collection(ctx, model.Map{}.TableName()).DeleteMany(ctx, bson.M{"map_name": mapName})
	if err != nil && !errors.Is(err, mongo.ErrNoDocuments) {
		log.ErrorfWithContext(ctx, "delete map from db error:%s", err.Error())
		return codes.ErrDatabase.Wrap(err)
	}
	_, errDeleteStandby := tm.db.Collection(ctx, model.StandbySite{}.TableName()).DeleteOne(ctx, bson.M{"map_name": mapName})
	if errDeleteStandby != nil && !errors.Is(errDeleteStandby, mongo.ErrNoDocuments) {
		log.ErrorfWithContext(ctx, "delete map standby point from db error:%s", errDeleteStandby.Error())
		return codes.ErrDatabase.Wrap(errDeleteStandby)
	}
	delete(tm.mapNameMapping, mapName)
	return nil
}

//AddLockSiteToDB 添加锁定站点到数据库
func (tm *TrafficManager) AddLockSiteToDB(ctx context.Context, mapName, siteID string) codes.Code {
	lockedSite := &model.LockedSite{
		MapName: mapName,
		SiteId:  siteID,
	}
	find := tm.MapNameExists(mapName)
	if !find {
		return codes.ErrMapNotFound
	}
	planer, find := tm.planers[mapName]
	if !find {
		return codes.ErrMapPlanerNotFound
	}
	siteLocks, err := planer.getAllLocalSiteFromDB(ctx)
	if err != nil {
		return err
	}
	for _, siteLock := range siteLocks {
		if siteLock == siteID {
			return codes.ErrNodeLocked
		}
	}
	dbRes := tm.db.Collection(ctx, model.LockedSite{}.TableName()).FindOneAndReplace(ctx, bson.M{"map_name": mapName, "site_id": siteID}, lockedSite, &options.FindOneAndReplaceOptions{Upsert: function.BoolPtr(true)})
	if dbRes.Err() != nil && !errors.Is(dbRes.Err(), mongo.ErrNoDocuments) {
		log.ErrorfWithContext(ctx, "add lock site to db error:%s", dbRes.Err().Error())
		return codes.ErrDatabase.Wrap(dbRes.Err())
	}
	return nil
}

//AddLockSiteToDB 添加区域锁定站点到数据库
func (tm *TrafficManager) AddLockAreaToDB(ctx context.Context, mapName string, id string, mainTaskCode string, nomalTaskOccupy *model.NomalTaskOccupy, normalTaskLock *sync.Mutex) codes.Code {
	tm.areaLocker.Lock()
	defer tm.areaLocker.Unlock()
	find := tm.MapNameExists(mapName)
	if !find {
		return codes.ErrMapNotFound
	}

	planer, find := tm.planers[mapName]
	if !find {
		return codes.ErrMapPlanerNotFound
	}
	getArea := func() *model.Area {
		for _, area := range planer.m.MapData.Areas {
			if area.Id == id && area.Type == model.AreaTypeInteractive {
				return &area
			}
		}
		return nil
	}
	area := getArea()
	if area == nil {
		return codes.ErrMapNotFound
	}
	//查询是否已存在
	res1 := tm.db.Collection(ctx, model.LockedArea{}.TableName()).FindOne(ctx, bson.M{"map_name": mapName, "area_id": area.Id})
	if res1.Err() != nil && !errors.Is(res1.Err(), mongo.ErrNoDocuments) {
		log.ErrorfWithContext(ctx, "get localArea from db error:%s", res1.Err().Error())
		return codes.ErrDatabase.Wrap(res1.Err())
	}
	localArea1 := new(model.LockedArea)
	_ = res1.Decode(localArea1)

	if localArea1.AreaId == id && localArea1.MapName == mapName {
		if _, ok := planer.occupiedArea[id]; !ok {
			var occupiedObj occupiedAreaObj
			occupiedObj.id = id
			occupiedObj.mainTaskCode = mainTaskCode
			planer.occupiedArea[id] = occupiedObj
		}
		log.Warnf("第三方交互区域（地图名：%s 区域ID：%d）,已经锁定", mapName, id)
		return nil
	}
	//判断是否在处理局部路径规划
	// if planer.localPathLockerFlag {
	// 	log.Warnf("存在正在规划的局部路径，不可锁定第三方交互区域（地图名：%s,区域ID：%d ）", mapName, id)
	// 	return codes.ErrDoingLocalPlan
	// }
	//判断当前规划器是否正在运行，正在运行
	if nomalTaskOccupy != nil && nomalTaskOccupy.GetOccupy() {
		log.Warnf("存在正在下发和规划的路径，不可执行第三方交互区域锁定操作！")
		return codes.ErrDoingLocalPlan
	}
	//先判断是否为空
	if normalTaskLock == nil {
		normalTaskLock = new(sync.Mutex)
	}
	// 处理第三方交互区域逻辑----------------------
	// planer.localPathLocker.Lock()
	// defer planer.localPathLocker.Unlock()
	normalTaskLock.Lock()
	defer normalTaskLock.Unlock()

	siteLocks, err := planer.getAllLocalSiteFromDB(ctx)
	if err != nil {
		return err
	}
	for _, siteLock := range siteLocks {
		for _, site := range area.ContainDotIds {
			if siteLock == site {
				return codes.ErrNodeLocked
			}
		}
	}

	lockedArea := &model.LockedArea{
		MapName:       mapName,
		AreaId:        area.Id,
		ContainDotIds: area.ContainDotIds,
		MainTaskCode:  mainTaskCode,
	}

	dbRes := tm.db.Collection(ctx, model.LockedArea{}.TableName()).FindOneAndReplace(ctx, bson.M{"map_name": mapName, "area_id": area.Id}, lockedArea, &options.FindOneAndReplaceOptions{Upsert: function.BoolPtr(true)})
	if dbRes.Err() != nil && !errors.Is(dbRes.Err(), mongo.ErrNoDocuments) {
		log.ErrorfWithContext(ctx, "add lock area to db error:%s", dbRes.Err().Error())
		return codes.ErrDatabase.Wrap(dbRes.Err())
	}
	var occupiedObj occupiedAreaObj
	occupiedObj.id = id
	occupiedObj.mainTaskCode = mainTaskCode
	planer.occupiedArea[id] = occupiedObj
	return nil
}

//DeleteLockSiteFromDB 从数据库删除锁定站点
func (tm *TrafficManager) DeleteLockSiteFromDB(ctx context.Context, mapName, siteID string) codes.Code {
	_, err := tm.db.Collection(ctx, model.LockedSite{}.TableName()).DeleteOne(ctx, bson.M{"map_name": mapName, "site_id": siteID})
	if err != nil {
		log.ErrorfWithContext(ctx, "delete lock site from db error:%s", err.Error())
		return codes.ErrDatabase.Wrap(err)
	}
	return nil
}

//DeleteLockSiteFromDB 从数据库删除锁定区域站点
func (tm *TrafficManager) DeleteLockAreaFromDB(ctx context.Context, mapName string, id string) codes.Code {
	tm.areaLocker.Lock()
	defer tm.areaLocker.Unlock()
	find := tm.MapNameExists(mapName)
	if !find {
		return codes.ErrMapNotFound
	}
	planer, find := tm.planers[mapName]
	if !find {
		return codes.ErrMapPlanerNotFound
	}
	_, err := tm.db.Collection(ctx, model.LockedArea{}.TableName()).DeleteOne(ctx, bson.M{"map_name": mapName, "area_id": id})
	if err != nil {
		log.ErrorfWithContext(ctx, "delete lock site from db error:%s", err.Error())
		return codes.ErrDatabase.Wrap(err)
	}
	delete(planer.occupiedArea, id)
	return nil
}

//GetAllLockSiteFromDB 从数据库中获得所有节点锁
func (tm *TrafficManager) GetAllLockSiteFromDB(ctx context.Context) ([]model.LockedSite, codes.Code) {
	sites := make([]model.LockedSite, 0)
	res, err := tm.db.Collection(ctx, model.LockedSite{}.TableName()).Find(ctx, bson.M{})
	if err != nil {
		log.ErrorfWithContext(ctx, "get all lock sites from db error:%s", err.Error())
		return nil, codes.ErrDatabase.Wrap(err)
	}
	if errBind := res.All(ctx, &sites); errBind != nil {
		log.ErrorfWithContext(ctx, "bind lock sites error:%s", errBind.Error())
		return nil, codes.ErrDatabase.Wrap(errBind)
	}
	return sites, nil
}

//GetAllLockSiteFromDB 从数据库中获得所有节点锁
func (tm *TrafficManager) GetAllLockAreaFromDB(ctx context.Context) ([]model.LockedArea, codes.Code) {
	// tm.areaLocker.Lock()
	// defer tm.areaLocker.Unlock()
	areas := make([]model.LockedArea, 0)
	res, err := tm.db.Collection(ctx, model.LockedArea{}.TableName()).Find(ctx, bson.M{})
	if err != nil {
		log.ErrorfWithContext(ctx, "get all lock sites from db error:%s", err.Error())
		return nil, codes.ErrDatabase.Wrap(err)
	}
	if errBind := res.All(ctx, &areas); errBind != nil {
		log.ErrorfWithContext(ctx, "bind lock sites error:%s", errBind.Error())
		return nil, codes.ErrDatabase.Wrap(errBind)
	}
	return areas, nil
}

//GetMapLockSiteFromDB 从数据库中获得指定地图的节点锁
func (tm *TrafficManager) GetMapLockSiteFromDB(ctx context.Context, mapName string) ([]model.LockedSite, codes.Code) {
	sites := make([]model.LockedSite, 0)
	res, err := tm.db.Collection(ctx, model.LockedSite{}.TableName()).Find(ctx, bson.M{"map_name": mapName})
	if err != nil {
		log.ErrorfWithContext(ctx, "get map lock sites from db error:%s", err.Error())
		return nil, codes.ErrDatabase.Wrap(err)
	}
	if errBind := res.All(ctx, &sites); errBind != nil {
		log.ErrorfWithContext(ctx, "bind lock sites error:%s", errBind.Error())
		return nil, codes.ErrDatabase.Wrap(errBind)
	}
	return sites, nil
}

//GetMapLockSiteFromDB 从数据库中获得指定地图的区域锁
func (tm *TrafficManager) GetMapLockAreaFromDB(ctx context.Context, mapName string) ([]model.LockedArea, codes.Code) {
	// tm.areaLocker.Lock()
	// defer tm.areaLocker.Unlock()
	areas := make([]model.LockedArea, 0)
	res, err := tm.db.Collection(ctx, model.LockedArea{}.TableName()).Find(ctx, bson.M{"map_name": mapName})
	if err != nil {
		log.ErrorfWithContext(ctx, "get map lock areas from db error:%s", err.Error())
		return nil, codes.ErrDatabase.Wrap(err)
	}
	if errBind := res.All(ctx, &areas); errBind != nil {
		log.ErrorfWithContext(ctx, "bind lock sites error:%s", errBind.Error())
		return nil, codes.ErrDatabase.Wrap(errBind)
	}
	return areas, nil
}

//MallocStandByPoint 申请待命点
func (tm *TrafficManager) MallocStandByPoint(ctx context.Context, robot *model.Robot, allRobots map[string]*model.Robot, rejectAll map[string]string, taskmanager *taskmanager.TaskManager) *model.StandbySite {
	type taskObj struct {
		taskMapName string
		taskDesSite string
	}
	tm.mapLocker.Lock()
	defer tm.mapLocker.Unlock()
	//机器人是否已经占用待命点
	dbRes := tm.db.Collection(ctx, model.StandbySite{}.TableName()).FindOne(ctx, bson.M{"agv_id": robot.Account.AgvId})
	if dbRes.Err() != nil && !errors.Is(dbRes.Err(), mongo.ErrNoDocuments) {
		log.ErrorfWithContext(ctx, "get robot standby site from db error:%s", dbRes.Err().Error())
		return nil
	}
	if dbRes.Err() == nil {
		site := new(model.StandbySite)
		if errBind := dbRes.Decode(site); errBind != nil {
			log.ErrorfWithContext(ctx, "bind robot standby site from db error:%s", errBind.Error())
			return nil
		}
		//todo 多楼层问题
		if !robot.StatusIsExists() || site.MapName == robot.Status.MapName {
			log.Infof("批复机器人:%s原有待命点:%s->%s", site.AgvID, site.MapName, site.SiteId)
			return site
		}
	}
	//当前车在待命点
	// if robot.StatusIsExists() && tm.planers[robot.Status.MapName].GetNode(robot.Status.SiteIdS).Type == model.NodeWaiting {
	// 	return nil
	// }
	sites := make([]*model.StandbySite, 0)
	res, err := tm.db.Collection(ctx, model.StandbySite{}.TableName()).Find(ctx, bson.M{"map_name": robot.Account.MapName, "agv_id": ""})
	if err != nil {
		log.ErrorfWithContext(ctx, "get map standby sites from db error:%s", err.Error())
		return nil
	}
	if errBind := res.All(ctx, &sites); errBind != nil {
		log.ErrorfWithContext(ctx, "bind lock sites error:%s", errBind.Error())
		return nil
	}
	var (
		bestStandbyPoint *model.StandbySite
		minDis           float64
	)
	//获取所有站点锁定信息
	lockSites, err := tm.GetAllLockSiteFromDB(ctx)
	if err != nil {
		log.Errorf("申请待命点时获取所有站点锁定信息失败")
		return nil
	}
	lockAreas, err := tm.GetAllLockAreaFromDB(ctx)
	if err != nil {
		log.Errorf("申请待命点时获取所有站点锁定信息失败")
		return nil
	}
	//获取未完成的充电点
	unfinishRechargerTasks, err1 := tm.taskManager.GetUnFinishedRechargerTasksFromDB(ctx, robot.Account.AgvId, robot.Account.MapName)
	if err1 != nil {
		log.Errorf("获得所有未完成的充电任务出错！！！")
		return nil
	}
	for _, rejectTask := range unfinishRechargerTasks {
		node1 := tm.GetNode1(robot.Account.MapName, rejectTask.TargetSiteID)
		if node1 == nil {
			continue
		}
		rejectAll[node1.Id] = node1.Id
		for _, ban := range node1.BanNodes {
			rejectAll[ban] = ban
		}
	}
	//获得正在执行任务死胡同
	// taskmanager.GetAllStatusTasksFromDB
	// allRunningTask, errCode := taskmanager.GetAllStatusTasksFromDB(ctx, model.TaskRunStatus())
	// if errCode != nil {
	// 	log.Panicf("get all running tasks error:%s", errCode.UnWrapError())
	// }

	// allTaskDataArr := make([]taskObj, 0)
	// for _, task := range allRunningTask {
	// 	//同一个地图点的集合
	// 	allTaskDataArr = append(allTaskDataArr, taskObj{
	// 		taskMapName: task.MapName,
	// 		taskDesSite: task.TargetSiteID,
	// 	})
	// }
	// //获得其它AGV当前所在的位置
	// for _, otherAgv := range allRobots {
	// 	if otherAgv.Account.AgvId == robot.Account.AgvId {
	// 		continue
	// 	}
	// 	if otherAgv.StatusIsExists() {
	// 		//记录其它所有车的位置
	// 		allTaskDataArr = append(allTaskDataArr, taskObj{
	// 			taskMapName: otherAgv.Status.MapName,
	// 			taskDesSite: otherAgv.Status.SiteIdS,
	// 		})
	// 	}
	// }
	//判断目前不可到达
	for _, site := range sites {
		if !robot.StatusIsExists() || site.MapName != robot.Status.MapName {
			continue
		}
		//判断待命点是否禁用此AGV
		if len(site.BanAgvList) > 0 {
			agvIsBan := false
			for _, banAgv := range site.BanAgvList {
				if banAgv == robot.Account.AgvId {
					agvIsBan = true
					break
				}
			}
			if agvIsBan {
				continue
			}
		}
		//判断该待命点是否允许该组
		if site.AllowGroupList != nil || len(site.AllowGroupList) > 0 {
			if !tools.StringSliceIsContain(site.AllowGroupList, robot.Account.GroupID) {
				continue
			}
		}
		//判断点是否已经锁定
		if site.In(lockSites) {
			continue
		}
		for _, lockArea := range lockAreas {
			if lockArea.MapName == site.MapName {
				for _, siteId := range lockArea.ContainDotIds {
					if site.SiteId == siteId {
						continue
					}
				}
			}
		}
		// isCanNotGoflag := false
		// //判断当前点是否在正在执行任务死胡同集合中
		// for _, taskObj1 := range allTaskDataArr {
		// 	if taskObj1.taskMapName == site.MapName {
		// 		//判断是否在死胡同，目标点所在死胡同内是否包含该待命点
		// 		if tm.planers[site.MapName].IsDeadWayAreaCanNotGo(taskObj1.taskDesSite, site) {
		// 			isCanNotGoflag = true
		// 			break
		// 		}
		// 		if tm.planers[site.MapName].IsMutexAreaCanNotGo(taskObj1.taskDesSite, site) {
		// 			isCanNotGoflag = true
		// 			break
		// 		}

		// 	}
		// }
		// if isCanNotGoflag {
		// 	continue
		// }
		//多楼层
		ctxTx := boot.Begin(ctx)
		gPath, errGPlan, _ := tm.planers[site.MapName].GlobalPathPlan(ctx, robot, robot.Status.SiteIdS, site.SiteId, site.SiteId, rejectAll, false, false, false)
		ctxTx.Rollback()
		//判断小车是否可以可以到达
		if errGPlan != nil {
			continue
		}
		if bestStandbyPoint == nil || gPath.Cost < minDis {
			bestStandbyPoint = site
			minDis = gPath.Cost
		}
	}
	if bestStandbyPoint != nil {
		_, errUpdate := tm.db.Collection(ctx, model.StandbySite{}.TableName()).UpdateOne(ctx, bson.M{"map_name": bestStandbyPoint.MapName, "site_id": bestStandbyPoint.SiteId}, bson.M{"$set": bson.M{"agv_id": robot.Account.AgvId}})
		if errUpdate != nil {
			log.ErrorfWithContext(ctx, "update map standby sites from db error:%s", errUpdate.Error())
			return nil
		}
		log.Infof("批复机器人:%s待命点:%s->%s", robot.Account.AgvId, bestStandbyPoint.MapName, bestStandbyPoint.SiteId)
	}
	return bestStandbyPoint
}

// 获取所有待命点
func (tm *TrafficManager) GetStandbySites(ctx context.Context) ([]model.StandbySite, codes.Code) {
	res, err := tm.db.Collection(ctx, model.StandbySite{}.TableName()).Find(ctx, bson.M{})
	if err != nil && !errors.Is(err, mongo.ErrNoDocuments) {
		log.ErrorfWithContext(ctx, "数据库获取待命点发生错误:%s", err.Error())
		return nil, codes.ErrDatabase.Wrap(err)
	}
	if err != nil && errors.Is(err, mongo.ErrNoDocuments) {
		return nil, nil
	}
	sites := make([]model.StandbySite, 0)
	if errBind := res.All(ctx, &sites); errBind != nil {
		log.ErrorfWithContext(ctx, "bind robot standby site from db error:%s", errBind.Error())
		return nil, codes.ErrDatabase.WithDetails(errBind.Error())
	}
	return sites, nil
}

// 获取待命点
func (tm *TrafficManager) GetStandbySite(ctx context.Context, mapName, siteId string) (*model.StandbySite, codes.Code) {
	dbRes := tm.db.Collection(ctx, model.StandbySite{}.TableName()).FindOne(ctx, bson.M{"map_name": mapName, "site_id": siteId})
	if dbRes.Err() != nil && !errors.Is(dbRes.Err(), mongo.ErrNoDocuments) {
		log.ErrorfWithContext(ctx, "数据库获取待命点发生错误:%s", dbRes.Err().Error())
		return nil, codes.ErrDatabase.Wrap(dbRes.Err())
	}
	if dbRes.Err() != nil && errors.Is(dbRes.Err(), mongo.ErrNoDocuments) {
		log.ErrorfWithContext(ctx, "数据库获取未查到此待命点:%s", dbRes.Err().Error())
		return nil, codes.ErrDatabase.Wrap(dbRes.Err())
	}
	site := new(model.StandbySite)
	if errBind := dbRes.Decode(site); errBind != nil {
		log.ErrorfWithContext(ctx, "bind robot standby site from db error:%s", errBind.Error())
		return nil, codes.ErrDatabase.WithDetails(errBind.Error())
	}
	return site, nil
}

// 修改禁用AGV列表---standBy
func (tm *TrafficManager) SaveStandbySiteAboutBandAgvListToDB(ctx context.Context, mapName, siteId string, banAgvList []string) codes.Code {
	tm.saveStandBySiteLocker.Lock()
	defer tm.saveStandBySiteLocker.Unlock()
	dbRes := tm.db.Collection(ctx, model.StandbySite{}.TableName()).FindOne(ctx, bson.M{"map_name": mapName, "site_id": siteId})
	if dbRes.Err() != nil && !errors.Is(dbRes.Err(), mongo.ErrNoDocuments) {
		log.ErrorfWithContext(ctx, "数据库获取待命点发生错误:%s", dbRes.Err().Error())
		return codes.ErrDatabase.Wrap(dbRes.Err())
	}
	if dbRes.Err() != nil && errors.Is(dbRes.Err(), mongo.ErrNoDocuments) {
		log.ErrorfWithContext(ctx, "数据库获取未查到此待命点:%s", dbRes.Err().Error())
		return codes.ErrDatabase.Wrap(dbRes.Err())
	}
	site := new(model.StandbySite)
	if errBind := dbRes.Decode(site); errBind != nil {
		log.ErrorfWithContext(ctx, "bind robot standby site from db error:%s", errBind.Error())
		return codes.ErrDatabase.WithDetails(errBind.Error())
	}

	site.BanAgvList = banAgvList
	dbRes = tm.db.Collection(ctx, model.StandbySite{}.TableName()).FindOneAndReplace(ctx, bson.M{"map_name": mapName, "site_id": siteId}, site, &options.FindOneAndReplaceOptions{Upsert: function.BoolPtr(true)})
	if dbRes.Err() != nil {
		log.ErrorfWithContext(ctx, "数据库保存待命点发生错误:%s", dbRes.Err().Error())
		return codes.ErrDatabase.Wrap(dbRes.Err())
	}
	return nil
}

//
func (tm *TrafficManager) SaveStandbySiteGroupListToDB(ctx context.Context, mapName, siteId string, groupList []string) codes.Code {
	tm.saveStandBySiteLocker.Lock()
	defer tm.saveStandBySiteLocker.Unlock()
	dbRes := tm.db.Collection(ctx, model.StandbySite{}.TableName()).FindOne(ctx, bson.M{"map_name": mapName, "site_id": siteId})
	if dbRes.Err() != nil && !errors.Is(dbRes.Err(), mongo.ErrNoDocuments) {
		log.ErrorfWithContext(ctx, "数据库获取待命点发生错误:%s", dbRes.Err().Error())
		return codes.ErrDatabase.Wrap(dbRes.Err())
	}
	if dbRes.Err() != nil && errors.Is(dbRes.Err(), mongo.ErrNoDocuments) {
		log.ErrorfWithContext(ctx, "数据库获取未查到此待命点:%s", dbRes.Err().Error())
		return codes.ErrDatabase.Wrap(dbRes.Err())
	}
	site := new(model.StandbySite)
	if errBind := dbRes.Decode(site); errBind != nil {
		log.ErrorfWithContext(ctx, "bind robot standby site from db error:%s", errBind.Error())
		return codes.ErrDatabase.WithDetails(errBind.Error())
	}

	site.AllowGroupList = groupList
	dbRes = tm.db.Collection(ctx, model.StandbySite{}.TableName()).FindOneAndReplace(ctx, bson.M{"map_name": mapName, "site_id": siteId}, site, &options.FindOneAndReplaceOptions{Upsert: function.BoolPtr(true)})
	if dbRes.Err() != nil {
		log.ErrorfWithContext(ctx, "数据库保存待命点发生错误:%s", dbRes.Err().Error())
		return codes.ErrDatabase.Wrap(dbRes.Err())
	}
	return nil
}

//LockRobotStandByPoint 锁定机器人占用的待命点
func (tm *TrafficManager) LockRobotStandByPoint(ctx context.Context, mapName, siteId string, agvID string) codes.Code {
	tm.standbySiteLocker.Lock()
	defer tm.standbySiteLocker.Unlock()

	_, errUpdate := tm.db.Collection(ctx, model.StandbySite{}.TableName()).UpdateOne(ctx, bson.M{"map_name": mapName, "site_id": siteId}, bson.M{"$set": bson.M{"agv_id": agvID}})
	if errUpdate != nil && !errors.Is(errUpdate, mongo.ErrNoDocuments) {
		log.ErrorfWithContext(ctx, "lock map standby sites from db error:%s", errUpdate.Error())
		return codes.ErrDatabase.Wrap(errUpdate)
	}
	if errUpdate != nil && errors.Is(errUpdate, mongo.ErrNoDocuments) {
		return codes.ErrRobotStandByNodeNotFound
	}
	return nil
}

//FreeRobotStandByPoint 释放机器人占用的待命点
func (tm *TrafficManager) FreeRobotStandByPoint(ctx context.Context, agvID string) codes.Code {
	tm.standbySiteLocker.Lock()
	defer tm.standbySiteLocker.Unlock()
	_, errUpdate := tm.db.Collection(ctx, model.StandbySite{}.TableName()).UpdateOne(ctx, bson.M{"agv_id": agvID}, bson.M{"$set": bson.M{"agv_id": ""}})
	if errUpdate != nil {
		log.ErrorfWithContext(ctx, "free map standby sites from db error:%s", errUpdate.Error())
		return codes.ErrDatabase.Wrap(errUpdate)
	}
	return nil
}

//DeleteMapStandByPoint 删除地图待命点
func (tm *TrafficManager) DeleteMapStandByPoint(ctx context.Context, mapName string) codes.Code {
	tm.standbySiteLocker.Lock()
	defer tm.standbySiteLocker.Unlock()
	_, errUpdate := tm.db.Collection(ctx, model.StandbySite{}.TableName()).DeleteMany(ctx, bson.M{"map_name": mapName})
	if errUpdate != nil {
		log.ErrorfWithContext(ctx, "delete map standby sites from db error:%s", errUpdate.Error())
		return codes.ErrDatabase.Wrap(errUpdate)
	}
	return nil
}
func (tm *TrafficManager) GetPathPlanner(mapName string) *Planer {
	planer, errGetPlaner := tm.getPlaner(mapName)
	if errGetPlaner != nil {
		return nil
	}
	return planer
}
func (planer *Planer) GetConnectPaths(node model.Node) []model.Path {
	res := make([]model.Path, 0)
	for _, p := range node.ConnectPaths {
		l := planer.m.MapData.Paths[p]
		res = append(res, l)
	}
	return res
}
func (tm *TrafficManager) IsAgvNoLocalPath(mapName, agvId string) bool {
	planer, errGetPlaner := tm.getPlaner(mapName)
	if errGetPlaner != nil {
		return true
	}
	oldLocalPath, errGet := planer.getRobotLocalPaths(context.Background(), agvId)
	if errGet != nil && !errors.Is(errGet, codes.ErrLocalPathNotFound) {
		return true
	}
	if oldLocalPath != nil && oldLocalPath.Paths != nil {
		return false
	}
	return true
}
func (tm *TrafficManager) PlanOnce(ctx context.Context, robot *model.Robot, mapName, source, target, realTarget string, globalRejects, globalRejectsAll, localRejects map[string]string, saveToDB, walkBack, needDelReverse, avoidFlag bool) (*model.GlobalPath, *model.LocalPath, codes.Code, string) {
	planer, errGetPlaner := tm.getPlaner(mapName)
	if errGetPlaner != nil {
		log.Errorf("获取不到规划器%s", mapName)
		return nil, nil, errGetPlaner, ""
	}
	return planer.planOnce(ctx, robot, mapName, source, target, realTarget, globalRejects, globalRejectsAll, localRejects, saveToDB, walkBack, needDelReverse, avoidFlag, false)
}
func (tm *TrafficManager) PathPlan(ctx context.Context, robot *model.Robot, subtask *model.Subtask, globalRejects, globalRejectsAll, localRejects map[string]string, useAgv, avoidFlag, saveToDB, retry bool) (*model.GlobalPath, *model.LocalPath, model.TaskSubStatus, string, int, codes.Code, string, *model.GlobalPath) {
	planer, errGetPlaner := tm.getPlaner(subtask.MapName)
	if errGetPlaner != nil {
		log.Errorf("获取不到规划器%s", subtask.MapName)
		return nil, nil, model.InitState, "0", 0, errGetPlaner, "", nil
	}

	robots := robotmanager.GetAllRobotsInSameMapFromCache(robot.Status.MapName)
	if useAgv && tm.SystemConfig.TrafficCfg.UseAvoidOtherAgvs {
		for _, r := range robots {
			if r.StatusIsExists() {
				if r.Account.AgvId != robot.Account.AgvId && (r.Status.Speed == 0 || r.Status.Status == model.RobotStatusIdle || r.Status.Status == model.RobotStatusStandby) {
					pathIds := make([]string, 0)
					for _, p := range r.AgvCorverPaths {
						pathIds = append(pathIds, planer.m.MapData.Paths[p].PathId)
					}
					planer.SetHeatingPathID(10, "", r.Account.AgvId, pathIds)
				}
			}

		}
	}

	gPath, lPath, planStatus, tmpTarget, tmpTargetLayer, errGPlan, blockAgv, gPurePaths := planer.PathPlanNew(ctx, robot, subtask, globalRejects, globalRejectsAll, localRejects, avoidFlag, saveToDB, retry)
	if useAgv && tm.SystemConfig.TrafficCfg.UseAvoidOtherAgvs {
		for _, r := range robots {
			if r.StatusIsExists() && r.Account.AgvId != robot.Account.AgvId && (r.Status.Speed == 0 || r.Status.Status == model.RobotStatusIdle || r.Status.Status == model.RobotStatusStandby) {
				pathIds := make([]string, 0)
				for _, p := range r.AgvCorverPaths {
					pathIds = append(pathIds, planer.m.MapData.Paths[p].PathId)
				}
				planer.removePathIDHeat(pathIds)
			}
		}
	}
	return gPath, lPath, planStatus, tmpTarget, tmpTargetLayer, errGPlan, blockAgv, gPurePaths
}
func (tm *TrafficManager) PathPlanBlock(ctx context.Context, robot *model.Robot, subtask *model.Subtask, globalRejects, globalRejectsAll, localRejects map[string]string) (*model.GlobalPath, *model.LocalPath, model.TaskSubStatus, string, int, codes.Code, string) {
	planer, errGetPlaner := tm.getPlaner(subtask.MapName)
	if errGetPlaner != nil {
		return nil, nil, model.InitState, "0", -1, errGetPlaner, ""
	}
	return planer.PathPlanBlock(ctx, robot, subtask, globalRejects, globalRejectsAll, localRejects)
}
func (tm *TrafficManager) GetNextSubTaskCost(mapName, targetSiteId string, robot *model.Robot) (*model.GlobalPath, codes.Code, *model.TargetCarPose) {
	planer, errGetPlaner := tm.getPlaner(mapName)
	if errGetPlaner != nil {
		return nil, errGetPlaner, nil
	}
	return planer.GetBestGlobalPathFullCost(context.Background(), robot, robot.Status.SiteIdS, targetSiteId, nil)
}
func (tm *TrafficManager) GetIdleAgvNearAvoid(ctx context.Context, robot *model.Robot, robots []*model.Robot, agvSite string, globalRejects, globalRejectsAll, localRejects, goalRejects map[string]string, freeAvoidRejects map[string]string, gPaths model.TrafficPaths) model.Node {
	planer, errGetPlaner := tm.getPlaner(robot.Account.MapName)
	if errGetPlaner != nil {
		return model.Node{}
	}
	return planer.getIdleAgvNearAvoidList(ctx, robot, robots, agvSite, globalRejects, globalRejectsAll, localRejects, goalRejects, freeAvoidRejects, gPaths)
}
func (tm *TrafficManager) GetAgvBestGlobalPath(ctx context.Context, robot *model.Robot, mapName, source, target string, globalRejects map[string]string, blockPath model.TrafficPaths) (map[string]string, map[string]string, model.TrafficPaths) {
	maxNum := func(a, b int) int {
		if a >= b {
			return a
		} else {
			return b
		}
	}
	goalRejects := make(map[string]string, 0)
	pathRejects := make(map[string]string, 0)
	planer, errGetPlaner := tm.getPlaner(mapName)
	if errGetPlaner != nil {
		return goalRejects, pathRejects, model.TrafficPaths{}
	}
	paths := make(model.TrafficPaths, 0)
	gotBlockAgv := false
	if blockPath != nil && len(blockPath.ToIDs()) > 0 {
		paths = blockPath
		gotBlockAgv = true
	} else {
		gPath, _ := planer.GetRobotGlobalPaths(ctx, robot.Account.AgvId)
		if gPath == nil || gPath.Paths == nil {
			gPath, _ = planer.getGlobalPath(ctx, robot, source, target, globalRejects)
		}
		if gPath != nil && gPath.Paths != nil {
			paths = gPath.Paths
		}
	}
	allApprovedLocalPaths, _ := planer.GetAllLocalPathFromDB(ctx)
	nodeNum := maxNum(robot.Type.LocalPathNodeNum, robot.LocalPathNum)
	for index, p := range paths {
		for _, site := range p.AllBlocks().ToIds() {
			goalRejects[site] = site
			if index < nodeNum+robot.Type.LocalPathNodeNum || index > len(paths)-3 || gotBlockAgv {
				pathRejects[site] = site
			}
		}
		for _, approved := range allApprovedLocalPaths {
			if approved.Sn == robot.Account.AgvId || approved.Paths != nil {
				continue
			}
			isConflict, _ := approved.Conflict(p, planer.getConnectPaths, false, robot)
			if !isConflict {
				//旋转判断车体扫过扇形区域与静止车辆的矩形区域是否相交
				isConflict, _ = approved.RotateConflictRect(p, robot)
			}
			if isConflict {
				r := robotmanager.GetFromCache(approved.Sn)
				for _, site := range r.AgvCorverNodes {
					// for _, site := range approved.SpecialLocks.ToIds() {
					goalRejects[site] = site
					if index < nodeNum+robot.Type.LocalPathNodeNum || index > len(paths)-3 || gotBlockAgv {
						pathRejects[site] = site
					}
				}
			}
		}
	}
	return goalRejects, pathRejects, paths
}

//GlobalPathPlan 全局路径规划
func (tm *TrafficManager) GlobalPathPlan(ctx context.Context, robot *model.Robot, mapName, source, target string) (*model.GlobalPath, bool, codes.Code) {
	planer, errGetPlaner := tm.getPlaner(mapName)
	if errGetPlaner != nil {
		return nil, false, errGetPlaner
	}
	gPaths, errGPlan, _ := planer.GlobalPathPlan(ctx, robot, source, target, target, nil, false, false, false)
	if errGPlan == nil {
		return gPaths, false, nil
	}
	if errGPlan != nil && !errors.Is(errGPlan, codes.ErrPathPlanNoGlobalPath) {
		return nil, false, errGPlan
	}
	//中转点尝试
	targetNode := planer.GetNode(target)
	//存在中转点,且机器人不在中转点
	if targetNode.IntermediateNode != "" {
		log.InfofWithContext(ctx, "目标点:%d 存在中转点:%d", targetNode.Id, targetNode.IntermediateNode)
	} else {
		log.InfofWithContext(ctx, "目标点:%d 中转点不存在", targetNode.Id)
	}
	if targetNode.IntermediateNode != "" && source != targetNode.IntermediateNode {
		gPaths, errGPlan, _ = planer.GlobalPathPlan(ctx, robot, source, targetNode.IntermediateNode, target, nil, false, false, false)
		if errGPlan != nil && !errors.Is(errGPlan, codes.ErrPathPlanNoGlobalPath) {
			return nil, false, errGPlan
		}
		if errGPlan != nil && errors.Is(errGPlan, codes.ErrPathPlanNoGlobalPath) {
			return nil, true, codes.ErrPathPlanNoGlobalPath.Wrap(codes.ErrGlobalTransmitPathNotFound)
		}
		return gPaths, true, nil
	}
	//存在中转点,且机器人在中转点
	if targetNode.IntermediateNode != "" && source == targetNode.IntermediateNode {
		log.InfofWithContext(ctx, "机器人正在中转点")
	}
	//不存在中转点
	return nil, false, codes.ErrPathPlanNoGlobalPath
}

//GetAllPathsFromDB 从数据库中获得所有路径规划结果
func (tm *TrafficManager) GetAllPathsFromDB() (map[string][]model.GlobalPath, map[string][]model.LocalPath) {
	ctx := context.Background()
	gRes := make(map[string][]model.GlobalPath, 0)
	lRes := make(map[string][]model.LocalPath, 0)
	for mapName, mapPlaner := range tm.planers {
		gPaths, _ := mapPlaner.getAllGlobalPaths(ctx)
		lPaths, _ := mapPlaner.GetAllLocalPathFromDB(ctx)
		gRes[mapName] = gPaths
		lRes[mapName] = lPaths
	}
	return gRes, lRes
}

//AddFailedPlanToDB 数据库中存储规划失败结果
func (tm *TrafficManager) AddFailedPlanToDB(ctx context.Context, agvID, blockAgv, mapName string, failedTaskType model.FailedTaskType) codes.Code {
	//检测数据库里是否已存在该小车记录
	dbRes := tm.db.Collection(ctx, model.FailedPathPlan{}.TableName()).FindOne(ctx, bson.M{"agv_id": agvID})
	if dbRes.Err() == nil {
		record := new(model.FailedPathPlan)
		if err := dbRes.Decode(record); err == nil {
			if record.BlockAgv != "" {
				log.Warnf("检测失败任务已存在robot:%s记录，不需要重新写入, record:%+v", agvID, record)
				return nil
			}
		}
	}

	record := model.FailedPathPlan{
		Uuid:           tools.GenUUID(),
		AgvID:          agvID,
		BlockAgv:       blockAgv,
		MapName:        mapName,
		FailedTaskType: failedTaskType,
		LastTime:       time.Now(),
	}
	errAdd := tm.db.Collection(ctx, model.FailedPathPlan{}.TableName()).FindOneAndReplace(ctx, bson.M{"agv_id": agvID}, record, &options.FindOneAndReplaceOptions{Upsert: function.BoolPtr(true)})
	if errAdd.Err() != nil && !errors.Is(errAdd.Err(), mongo.ErrNoDocuments) {
		log.Errorf("添加robot:%s记录失败 %s", agvID, errAdd.Err())
		return codes.ErrDatabase.Wrap(errAdd.Err())
	}
	return nil
}

//UpdateFailedPlanToDB 更新数据库中存储规划失败结果
func (tm *TrafficManager) UpdateFailedPlanToDB(ctx context.Context, uuid string) codes.Code {
	_, errUpdate := tm.db.Collection(ctx, model.FailedPathPlan{}.TableName()).UpdateOne(ctx, bson.M{"uuid": uuid}, bson.M{"$set": bson.M{"last_time": time.Now()}})
	if errUpdate != nil && !errors.Is(errUpdate, mongo.ErrNoDocuments) {
		log.ErrorfWithContext(ctx, "update failed plan to db error:%s", errUpdate.Error())
		return codes.ErrDatabase.Wrap(errUpdate)
	}
	return nil
}
func (tm *TrafficManager) UpdateFailedPlanAGVToDB(ctx context.Context, agv, block_agv string) codes.Code {
	_, errUpdate := tm.db.Collection(ctx, model.FailedPathPlan{}.TableName()).UpdateOne(ctx, bson.M{"agv_id": agv}, bson.M{"$set": bson.M{"block_agv": block_agv}})
	if errUpdate != nil && !errors.Is(errUpdate, mongo.ErrNoDocuments) {
		log.ErrorfWithContext(ctx, "update failed plan to db error:%s", errUpdate.Error())
		return codes.ErrDatabase.Wrap(errUpdate)
	}
	return nil
}

//DeleteFailedPlanFromDB 数据库中删除规划失败结果
func (tm *TrafficManager) DeleteFailedPlanFromDB(ctx context.Context, uuid string, robot *model.Robot) codes.Code {
	_, errAdd := tm.db.Collection(ctx, model.FailedPathPlan{}.TableName()).DeleteOne(ctx, bson.M{"uuid": uuid})
	if errAdd != nil {
		log.ErrorfWithContext(ctx, "delete failed plan from db error:%s", errAdd.Error())
		return codes.ErrDatabase.Wrap(errAdd)
	}
	if robot.StatusIsExists() {
		robot.Status.IsBlock = false
	}
	return nil
}

//GetAllFailedPlanToDB 从数据库中获得所有规划失败结果
func (tm *TrafficManager) GetAllFailedPlanToDB(ctx context.Context) ([]model.FailedPathPlan, codes.Code) {
	records := make([]model.FailedPathPlan, 0)
	res, errFind := tm.db.Collection(ctx, model.FailedPathPlan{}.TableName()).Find(ctx, bson.M{})
	if errFind != nil {
		log.ErrorfWithContext(ctx, "get all failed plan from db error:%s", errFind.Error())
		return nil, codes.ErrDatabase.Wrap(errFind)
	}
	if err := res.All(ctx, &records); err != nil {
		log.ErrorfWithContext(ctx, "bind failed plan error:%s", err.Error())
		return nil, nil
	}
	return records, nil
}
func (tm *TrafficManager) GetFailedPlanByBlockAgv(ctx context.Context, blockAgv string) (bool, []string) {
	agvs := make([]string, 0)
	records := make([]model.FailedPathPlan, 0)
	res, errFind := tm.db.Collection(ctx, model.FailedPathPlan{}.TableName()).Find(ctx, bson.M{"block_agv": blockAgv})
	if errFind != nil {
		log.ErrorfWithContext(ctx, "get all failed plan from db error:%s", errFind.Error())
		return false, agvs
	}
	if err := res.All(ctx, &records); err != nil {
		log.ErrorfWithContext(ctx, "bind failed plan error:%s", err.Error())
		return false, agvs
	}
	for _, r := range records {
		agvs = append(agvs, r.AgvID)
	}
	return true, agvs
}

//GetAllFailedPlanToDB 从数据库中获得指定地图规划失败结果
func (tm *TrafficManager) IsAgvInFailedPlanWait(ctx context.Context, agv string) (bool, string) {
	record := new(model.FailedPathPlan)
	dbRes := tm.db.Collection(ctx, model.FailedPathPlan{}.TableName()).FindOne(ctx, bson.M{"agv_id": agv})
	// if errors.Is(dbRes.Err(), mongo.ErrNoDocuments) {
	// 	return true, ""
	// } else
	if dbRes.Err() != nil {
		return false, ""
	}
	if err := dbRes.Decode(record); err != nil {
		return false, ""
	}
	if record.BlockAgv == "" {
		return true, ""
	}
	return false, record.BlockAgv
}
func (tm *TrafficManager) IsAgvInFailedPlanCycleWait(ctx context.Context, agv string) bool {
	inputAgv := agv
	agvList := make(map[string]string, 0)
	agvList[inputAgv] = inputAgv
	for {
		tools.Sleep(0.1)
		ok, blockAgv := tm.IsAgvInFailedPlanWait(ctx, inputAgv)
		if ok {
			return true
		} else {
			if _, find := agvList[blockAgv]; find {
				return false
			}
			if blockAgv != "" {
				inputAgv = blockAgv
				agvList[inputAgv] = inputAgv
				continue
			} else {
				return false
			}
		}
	}
}
func (tm *TrafficManager) GetMapFailedPlanToDB(ctx context.Context, mapName string) ([]model.FailedPathPlan, codes.Code) {
	records := make([]model.FailedPathPlan, 0)
	res, errFind := tm.db.Collection(ctx, model.FailedPathPlan{}.TableName()).Find(ctx, bson.M{"map_name": mapName})
	if errFind != nil {
		log.ErrorfWithContext(ctx, "get all failed plan from db error:%s", errFind.Error())
		return nil, codes.ErrDatabase.Wrap(errFind)
	}
	if err := res.All(ctx, &records); err != nil {
		log.ErrorfWithContext(ctx, "bind failed plan error:%s", err.Error())
		return nil, codes.ErrDatabase
	}
	return records, nil
}

//Reach 机器人到达点位时处理
func (tm *TrafficManager) Reach(ctx context.Context, robot *model.Robot, mapName string, pathID string, reachType proto.IdType, localRejects map[string]string) (*model.GlobalPath, *model.LocalPath, bool, string, codes.Code) {
	planer, errGetPlaner := tm.getPlaner(mapName)
	if errGetPlaner != nil {
		return nil, nil, false, "", errGetPlaner
	}
	return planer.Reach(ctx, robot, pathID, reachType, localRejects)
}

//DeleteRobotAllGlobalPaths 清除机器人所有全局路径
func (tm *TrafficManager) DeleteRobotAllGlobalPaths(ctx context.Context, sn string) codes.Code {
	for _, planer := range tm.planers {
		if err := planer.DeleteRobotGlobalPaths(ctx, sn, planer.m.MapName); err != nil {
			log.ErrorfWithContext(ctx, "delete robot:%s global path error:%s", sn, err.Error())
			return err
		}
	}
	return nil
}

//DeleteRobotAllLocalPaths 清除机器人所有局部路径和所有锁定点
func (tm *TrafficManager) DeleteRobotAllLocalPaths(ctx context.Context, sn string) codes.Code {
	for _, planer := range tm.planers {
		if err := planer.DeleteRobotLocalPaths(ctx, sn, planer.m.MapName); err != nil {
			log.ErrorfWithContext(ctx, "delete robot:%s local path error:%s", sn, err.Error())
			return err
		}
	}
	return nil
}

//DeleteRobotGlobalPaths 清除机器人全局路径
func (tm *TrafficManager) DeleteRobotGlobalPaths(ctx context.Context, agvId string, mapName string) codes.Code {
	planer, errGetPlaner := tm.getPlaner(mapName)
	if errGetPlaner != nil {
		return codes.ErrMapPlanerNotFound
	}
	return planer.DeleteRobotGlobalPaths(ctx, agvId, mapName)
}

//DeleteRobotLocalPaths 清除机器人局部路径和所有AGV锁定点
func (tm *TrafficManager) DeleteRobotLocalPaths(ctx context.Context, agvId string, mapName string) codes.Code {
	planer, errGetPlaner := tm.getPlaner(mapName)
	if errGetPlaner != nil {
		return codes.ErrMapPlanerNotFound
	}
	return planer.DeleteRobotLocalPaths(ctx, agvId, mapName)
}

//清除局部路径(只清除路径和路径锁，不清除锁特殊锁)
func (tm *TrafficManager) ClearRobotLocalPaths(ctx context.Context, agvId string, mapName string) codes.Code {
	planer, errGetPlaner := tm.getPlaner(mapName)
	if errGetPlaner != nil {
		return codes.ErrMapPlanerNotFound
	}
	return planer.ClearRobotLocalPaths(ctx, agvId, mapName)
}

//LockNodeAround 锁定指定节点一定范围内的点
func (tm *TrafficManager) LockNodeAround(ctx context.Context, robot *model.Robot, mapName, siteId string, angle int, robotType model.RobotType, isOccupyElevetor, isDoOnline bool) codes.Code {
	planer, errGetPlaner := tm.getPlaner(mapName)
	if errGetPlaner != nil {
		return codes.ErrMapPlanerNotFound
	}
	return planer.LockNodeAround(ctx, robot, mapName, siteId, angle, robotType, isOccupyElevetor, isDoOnline)
}

//清除AGV路径和对应锁定点（不清除AGV当前点锁）
func (tm *TrafficManager) ClearRobotPathAndLockNode(ctx context.Context, robot *model.Robot, mapName, siteId string, angle int, robotType model.RobotType) codes.Code {
	//1、清除全局路径
	err := tm.DeleteRobotGlobalPaths(ctx, robot.Account.AgvId, mapName)
	if err != nil {
		return err
	}
	//2、更新点锁定
	err = tm.LockNodeAround(ctx, robot, mapName, siteId, angle, robotType, false, false)
	if err != nil {
		return err
	}
	//3、清除局部路径(只清除路径和路径锁，不清除锁特殊锁)
	err = tm.ClearRobotLocalPaths(ctx, robot.Account.AgvId, mapName)
	if err != nil {
		return err
	}
	return nil
}

//getPlaner 获得交通规划器
func (tm *TrafficManager) getPlaner(mapName string) (*Planer, codes.Code) {
	planer, find := tm.planers[mapName]
	if !find {
		log.Warnf("planer not found:%s", mapName)
		return nil, codes.ErrMapPlanerNotFound
	}
	return planer, nil
}

func (tm *TrafficManager) refreshMapStandbyPoints(ctx context.Context, m *model.Map) codes.Code {
	newSites := make([]model.StandbySite, 0)
	for _, s := range m.MapData.NodesMap {
		if s.Type != model.NodeWaiting {
			continue
		}
		newSites = append(newSites, model.StandbySite{MapName: m.MapName, SiteId: s.Id})
	}
	sites := make([]model.StandbySite, 0)
	res, err := tm.db.Collection(ctx, model.StandbySite{}.TableName()).Find(ctx, bson.M{"map_name": m.MapName})
	if err != nil {
		log.ErrorfWithContext(ctx, "get map lock sites from db error:%s", err.Error())
		return codes.ErrDatabase.Wrap(err)
	}
	if errBind := res.All(ctx, &sites); errBind != nil {
		log.ErrorfWithContext(ctx, "bind lock sites error:%s", errBind.Error())
		return codes.ErrDatabase.Wrap(errBind)
	}
	for _, n := range newSites {
		find := false
		for _, o := range sites {
			if n.SiteId == o.SiteId {
				find = true
				break
			}
		}
		if !find {
			//在原来站点中没找到的话，创建新的
			_, errInsert := tm.db.Collection(ctx, model.StandbySite{}.TableName()).InsertOne(ctx, n)
			if errInsert != nil {
				log.ErrorfWithContext(ctx, "insert new standby point error:")
			}
		}
	}
	for _, o := range sites {
		find := false
		for _, n := range newSites {
			if n.SiteId == o.SiteId {
				find = true
				break
			}
		}
		if !find {
			//在新站点中没找到的话，删除
			if o.AgvID != "" {
				log.Warnf("移除待命点：地图名:%s,点位：%s 机器人:%s待命中", o.MapName, o.SiteId, o.AgvID)
			}
			_, errInsert := tm.db.Collection(ctx, model.StandbySite{}.TableName()).DeleteOne(ctx, bson.M{"map_name": o.MapName, "site_id": o.SiteId})
			if errInsert != nil {
				log.ErrorfWithContext(ctx, "insert new standby point error:")
			}
		}
	}
	return nil
}

func (tm *TrafficManager) Init() {
	maps, errGetMaps := tm.GetAllMapFromDB(context.TODO())
	if errGetMaps != nil {
		log.Panicf("init error,get maps error:%s", errGetMaps.UnWrapError())
		return
	}
	tm.planers = make(map[string]*Planer, 0)
	for _, m := range maps {
		if errRefresh := tm.refreshMapStandbyPoints(context.TODO(), m); errRefresh != nil {
			log.Panicf("refresh standby point error:%s", errRefresh.UnWrapError())
		}
		tm.mapNameMapping[m.MapName] = mapInfo{m.ProjectId, m.TrafficName}
		tm.devManager.Load(m.MapName, m.MapData.NodesMap)
		planer := newPlaner(m, tm.db, tm.devManager, tm.taskManager)
		areas, err := tm.GetMapLockAreaFromDB(context.Background(), m.MapName)
		if err == nil {
			for _, area := range areas {
				var occupiedObj occupiedAreaObj
				occupiedObj.id = area.AreaId
				occupiedObj.mainTaskCode = area.MainTaskCode
				planer.occupiedArea[area.AreaId] = occupiedObj
			}
		}
		tm.planers[m.MapName] = planer
		//从缓存获得地堆库数据
		locations := tm.locationManager.GetGroundLocationsFromCacheByMapName(context.TODO(), m.MapName)
		tm.InitLocationGroups(m.MapName, locations)
	}
	tm.crossPlaner.mapRelations, _ = tm.GetAllMapRelationFromDB(context.TODO())
	tm.crossPlaner.initDijkstraMap(maps)
}

func (tm *TrafficManager) ReInit(m *model.Map) {
	//初始化加载地堆库
	if errRefresh := tm.refreshMapStandbyPoints(context.TODO(), m); errRefresh != nil {
		log.Panicf("refresh standby point error:%s", errRefresh.UnWrapError())
	}
	tm.mapNameMapping[m.MapName] = mapInfo{m.ProjectId, m.TrafficName}
	tm.devManager.Load(m.MapName, m.MapData.NodesMap)
	planer := newPlaner(m, tm.db, tm.devManager, tm.taskManager)
	areas, err := tm.GetMapLockAreaFromDB(context.Background(), m.MapName)
	if err == nil {
		for _, area := range areas {
			var occupiedObj occupiedAreaObj
			occupiedObj.id = area.AreaId
			occupiedObj.mainTaskCode = area.MainTaskCode
			planer.occupiedArea[area.AreaId] = occupiedObj
		}
	}
	tm.planers[m.MapName] = planer
	//从缓存获得地堆库数据
	locations := tm.locationManager.GetGroundLocationsFromCacheByMapName(context.TODO(), m.MapName)
	tm.InitLocationGroups(m.MapName, locations)
	maps, errGetMaps := tm.GetAllMapFromDB(context.TODO())
	if errGetMaps != nil {
		log.Panicf("init error,get maps error:%s", errGetMaps.UnWrapError())
		return
	}
	tm.crossPlaner.mapRelations, _ = tm.GetAllMapRelationFromDB(context.TODO())
	tm.crossPlaner.initDijkstraMap(maps)
}

func (tm *TrafficManager) InitLocationGroups(mapName string, locations []*model.Location) {
	//整体更新,先获得
	if planerObj, find := tm.planers[mapName]; find {
		if locations == nil {
			location1s := make([]*model.Location, 0)
			planerObj.initLocationArea(location1s)
			return
		}
		planerObj.initLocationArea(locations)
	}

}

//获取点位的前一个点
func (tm *TrafficManager) GetTargetFront(mapName, siteId string, robot *model.Robot) (*model.Node, codes.Code) {
	planer, err := tm.getPlaner(mapName)
	if err != nil {
		log.Errorf("获取当前点位(%d)前一个点失败，获取交通规划器失败", siteId)
		return nil, err
	}
	pointInArea := func(myid string, ContainDotIds []string) bool {
		for _, id := range ContainDotIds {
			if id == myid {
				return true
			}
		}
		return false
	}
	inBanArea := func(frontSiteId string) bool {
		for _, a := range planer.m.MapData.Areas {
			if a.Type == model.AreaTypeBan {
				for _, r := range a.BanAgvIds {
					if r == robot.Account.AgvId && pointInArea(frontSiteId, a.ContainDotIds) {
						return true
					}
				}
			}
		}
		return false
	}
	node := planer.GetNode(siteId)
	if node.LocationType == model.LocationGround && node.Type == model.NodeStation {
		groundLocationGroup := tm.locationManager.FindGroundLocation(mapName, siteId)
		if groundLocationGroup == nil {
			return nil, codes.ErrMapNodeNotFound
		}
		frontNode := planer.GetTaskTargetNode(groundLocationGroup.FrontPointId)
		return frontNode, nil
	}
	paths := planer.getConnectPaths(node)
	CategoryIdStr := string(robot.Type.CategoryId)
	for _, p := range paths {
		isNotCanGo := false
		if CategoryIdStr != "" {
			if tools.StringSliceIsContain(strings.Split(p.Reserve1, ";"), CategoryIdStr) {
				isNotCanGo = true
			}
		}
		if isNotCanGo {
			continue
		}
		frontSite := p.GetAnother(node)
		if !inBanArea(frontSite.Id) {
			return &frontSite, nil
		}
	}
	return nil, codes.ErrMapNodeNotFound
}

// 申请称重平台点
func (tm *TrafficManager) MallocWeightPlatformSite(ctx context.Context, robot *model.Robot, mapName, targetSiteId string) (*model.Node, codes.Code) {
	tm.mapLocker.Lock()
	defer tm.mapLocker.Unlock()
	planer, err := tm.getPlaner(mapName)
	if err != nil {
		log.Errorf("申请称重平台失败，获取交通规划器失败！")
		return nil, err
	}
	nodes := make([]model.Node, 0)
	for _, node := range planer.m.MapData.NodesMap {
		if node.Type == model.NodePlatformWeight && node.DeviceID != "" && node.DeviceID != "string" {
			nodes = append(nodes, node)
		}
	}
	if len(nodes) == 0 {
		return nil, codes.ErrDevicePlatFormWightDotIsNull
	}
	//获取所有站点锁定信息
	lockSites, err := tm.GetAllLockSiteFromDB(ctx)
	if err != nil {
		log.Errorf("申请称重平台时获取所有站点锁定信息失败！")
		return nil, err
	}
	lockAreas, err := tm.GetAllLockAreaFromDB(ctx)
	if err != nil {
		log.Errorf("申请称重平台时获取所有站点锁定信息失败！")
		return nil, err
	}

	// isCurrentExisit := false
	isTargetExisit := false
	var (
		// bestCurrentPlatformWeightPoint model.Node //离当前agv最优的称重平台
		// minCurrentCost                 float64    //离当前agv最优的称重平台点的代价值
		bestTargetPlatformWeightPoint model.Node //离目标点最优的称重平台点
		minTargetCost                 float64    //离目标点最优的称重平台点的代价值
	)
	var targetCarBody *model.TargetCarPose
	if robot.Status.SiteIdS != targetSiteId {
		_, errGPlan, targetCarBody1 := planer.getBestGlobalPathCost(ctx, robot, robot.Status.SiteIdS, targetSiteId, nil)
		targetCarBody = targetCarBody1
		if errGPlan != nil {
			log.Errorf("robot:%s 到目标点 %s 不可达！！！", robot.Account.AgvId, targetSiteId)
			return nil, errGPlan
		}
	} else {
		targetCarBody = &model.TargetCarPose{
			TargetSiteId: robot.Status.SiteIdS,
			PathID:       "",
			CarBodyRad:   float64(robot.Status.Angle) * math.Pi / 180,
		}
	}

	// //深拷贝
	var robotCopy model.Robot
	temp, _ := json.Marshal(robot)
	json.Unmarshal(temp, &robotCopy)
	robotCopy.Status.Angle = int(targetCarBody.CarBodyRad * 180 / math.Pi)
	robotCopy.Status.SiteIdS = targetCarBody.TargetSiteId

	for _, node := range nodes {
		//判断点是否已经锁定
		if node.In(mapName, lockSites) {
			continue
		}
		for _, lockArea := range lockAreas {
			if lockArea.MapName == mapName {
				//当前车的
				for _, siteId := range lockArea.ContainDotIds {
					if node.Id == siteId {
						continue
					}
				}
			}
		}

		//判断小车是否可以可以到达,获取最近的旋转点
		//车位置->货叉旋转点
		// currentCost, errCurrentGPlan, _ := planer.getBestGlobalPathCost(ctx, robot, robot.Status.SiteIdS, node.Id, nil)
		//任务目标点->货物叉旋转点
		targetCost, errTargetGPlan, _ := planer.getBestGlobalPathCost(ctx, &robotCopy, targetSiteId, node.Id, nil)
		//当前AGV位置寻找的最近货叉旋转点
		// if errCurrentGPlan == nil {
		// 	if !isCurrentExisit || currentCost < minCurrentCost {
		// 		bestCurrentPlatformWeightPoint = node
		// 		minCurrentCost = currentCost
		// 		isCurrentExisit = true
		// 	}
		// }
		//当前目标点寻找的最近货叉旋转点
		if errTargetGPlan == nil {
			if !isTargetExisit || targetCost < minTargetCost {
				bestTargetPlatformWeightPoint = node
				minTargetCost = targetCost
				isTargetExisit = true
			}
		}
	}

	//都未找到货叉旋转点
	// if !isCurrentExisit && !isTargetExisit {
	// 	//log.Infof("获取最近的旋转点失败，未找到小车可以可以到达的旋转点  robot:%s", robot.Account.AgvId)
	// 	return nil, codes.ErrPathPlanNoGlobalPath
	// } else if !isCurrentExisit && isTargetExisit {
	// 	return &bestTargetPlatformWeightPoint, nil
	// } else if isCurrentExisit && !isTargetExisit {
	// 	return &bestCurrentPlatformWeightPoint, nil
	// }
	// //对比求和,2个都能找到
	// //1.终点->起点的代价值
	// cost1, errPlan1, _ := planer.getBestGlobalPathCost(ctx, robot, robot.Status.SiteIdS, bestTargetPlatformWeightPoint.Id, nil)

	// //2.起点找终点的代价值
	// cost2, errPlan2, _ := planer.getBestGlobalPathCost(ctx, &robotCopy, robotCopy.Status.SiteIdS, bestCurrentPlatformWeightPoint.Id, nil)
	// if errPlan1 != nil && errPlan2 != nil {
	// 	return nil, codes.ErrPathPlanNoGlobalPath
	// } else if errPlan1 == nil && errPlan2 != nil {
	// 	return &bestTargetPlatformWeightPoint, nil
	// } else if errPlan1 != nil && errPlan2 == nil {
	// 	return &bestCurrentPlatformWeightPoint, nil
	// }
	// //比较
	// if (minTargetCost + cost1) >= (cost2 + minCurrentCost) {
	// 	return &bestCurrentPlatformWeightPoint, nil
	// }

	return &bestTargetPlatformWeightPoint, nil
}

// 申请货叉旋转点
func (tm *TrafficManager) MallocRotatePoint(ctx context.Context, robot *model.Robot, mapName, targetSiteId string, nomalTaskLock *sync.Mutex) (*model.Node, codes.Code) {
	tm.mapLocker.Lock()
	defer tm.mapLocker.Unlock()
	planer, err := tm.getPlaner(mapName)
	if err != nil {
		log.Errorf("申请货叉旋转点失败，获取交通规划器失败")
		return nil, err
	}
	nodes := make([]model.Node, 0)
	for _, node := range planer.m.MapData.NodesMap {
		if node.ForkRotateFlag == 1 {
			nodes = append(nodes, node)
		}
	}

	//获取所有站点锁定信息
	lockSites, err := tm.GetAllLockSiteFromDB(ctx)
	if err != nil {
		log.Errorf("申请货叉旋转点时获取所有站点锁定信息失败")
		return nil, err
	}
	lockAreas, err := tm.GetAllLockAreaFromDB(ctx)
	if err != nil {
		log.Errorf("申请货叉旋转点时获取所有站点锁定信息失败")
		return nil, err
	}

	//找到当前AGV绑定的主任务编号
	bindMainTask, errb := tm.taskManager.GetMainTaskByAgvFromDB(ctx, robot.Account.AgvId)
	if errb != nil {
		log.Errorf("robot:%s,申请货叉旋转点时获取所有任务绑定信息失败！！", robot.Account.AgvId)
		return nil, errb
	}
	isCurrentExisit := false
	isTargetExisit := false
	var (
		bestCurrentRotatePoint model.Node //离当前agv最优的货叉旋转点
		minCurrentCost         float64    //离当前agv最优的货叉旋转点的代价值
		bestTargetRotatePoint  model.Node //离目标点最优的货叉旋转点
		minTargetCost          float64    //离目标点最优的货叉旋转点的代价值
	)
	var targetCarBody *model.TargetCarPose
	if robot.Status.SiteIdS != targetSiteId {
		_, errGPlan, targetCarBody1 := planer.getBestGlobalPathCost(ctx, robot, robot.Status.SiteIdS, targetSiteId, nil)
		targetCarBody = targetCarBody1
		if errGPlan != nil {
			log.Errorf("robot:%s 到目标点 %s 不可达！！！", robot.Account.AgvId, targetSiteId)
			return nil, errGPlan
		}
	} else {
		targetCarBody = &model.TargetCarPose{
			TargetSiteId: robot.Status.SiteIdS,
			PathID:       "",
			CarBodyRad:   float64(robot.Status.Angle) * math.Pi / 180,
		}
	}

	// //深拷贝
	var robotCopy model.Robot
	temp, _ := json.Marshal(robot)
	json.Unmarshal(temp, &robotCopy)
	robotCopy.Status.Angle = int(targetCarBody.CarBodyRad * 180 / math.Pi)
	robotCopy.Status.SiteIdS = targetCarBody.TargetSiteId

	for _, node := range nodes {
		//判断点是否已经锁定
		if node.In(mapName, lockSites) {
			continue
		}
		for _, lockArea := range lockAreas {
			if lockArea.MapName == mapName {
				//当前车的
				if bindMainTask != nil && lockArea.MainTaskCode != "" && bindMainTask.MainTaskCode != "" && lockArea.MainTaskCode == bindMainTask.MainTaskCode {
					continue
				}
				for _, siteId := range lockArea.ContainDotIds {
					if node.Id == siteId {
						continue
					}
				}
			}
		}

		//判断小车是否可以可以到达,获取最近的旋转点
		//车位置->货叉旋转点
		currentCost, errCurrentGPlan, _ := planer.getBestGlobalPathCost(ctx, robot, robot.Status.SiteIdS, node.Id, nil)
		//任务目标点->货物叉旋转点
		targetCost, errTargetGPlan, _ := planer.getBestGlobalPathCost(ctx, &robotCopy, targetSiteId, node.Id, nil)
		//当前AGV位置寻找的最近货叉旋转点
		if errCurrentGPlan == nil {
			if !isCurrentExisit || currentCost < minCurrentCost {
				bestCurrentRotatePoint = node
				minCurrentCost = currentCost
				isCurrentExisit = true
			}
		}
		//当前目标点寻找的最近货叉旋转点
		if errTargetGPlan == nil {
			if !isTargetExisit || targetCost < minTargetCost {
				bestTargetRotatePoint = node
				minTargetCost = targetCost
				isTargetExisit = true
			}
		}

	}

	//都未找到货叉旋转点
	if !isCurrentExisit && !isTargetExisit {
		log.Infof("获取最近的旋转点失败，未找到小车可以可以到达的旋转点  robot:%s", robot.Account.AgvId)
		return nil, codes.ErrPathPlanNoGlobalPath
	}
	//先判断是否为空
	if nomalTaskLock == nil {
		nomalTaskLock = new(sync.Mutex)
	}
	nomalTaskLock.Lock()
	siteLocks, err1 := planer.getAllOtherLocalSiteFromDB(ctx, robot.Account.AgvId)
	nomalTaskLock.Unlock()
	if err1 != nil {
		return nil, err1
	}
	rejPoints := make([]string, 0)
	rejPoints = siteLocks
	if len(rejPoints) > 0 {
		//判断货叉旋转点是否都可以去
		if isCurrentExisit && bestCurrentRotatePoint.Reserve1 != "" {
			//判断当前车最近的货叉旋转点
			currentArr := strings.Split(bestCurrentRotatePoint.Reserve1, ";")
			for _, value := range currentArr {
				if tools.StringSliceIsContain(rejPoints, value) {
					isCurrentExisit = false
					break
				}

			}

		}
		if isTargetExisit && bestTargetRotatePoint.Reserve1 != "" {
			//判断目标点的附加属性是否存在
			targetArr := strings.Split(bestTargetRotatePoint.Reserve1, ";")
			for _, value := range targetArr {
				if tools.StringSliceIsContain(rejPoints, value) {
					isTargetExisit = false
				}
			}
		}

	}
	if !isCurrentExisit && isTargetExisit {
		return &bestTargetRotatePoint, nil
	} else if isCurrentExisit && !isTargetExisit {
		return &bestCurrentRotatePoint, nil
	}
	//对比求和,2个都能找到
	//1.终点->起点的代价值
	cost1, errPlan1, _ := planer.getBestGlobalPathCost(ctx, robot, robot.Status.SiteIdS, bestTargetRotatePoint.Id, nil)

	//2.起点找终点的代价值
	cost2, errPlan2, _ := planer.getBestGlobalPathCost(ctx, &robotCopy, robotCopy.Status.SiteIdS, bestCurrentRotatePoint.Id, nil)
	if errPlan1 != nil && errPlan2 != nil {
		return nil, codes.ErrPathPlanNoGlobalPath
	} else if errPlan1 == nil && errPlan2 != nil {
		return &bestTargetRotatePoint, nil
	} else if errPlan1 != nil && errPlan2 == nil {
		return &bestCurrentRotatePoint, nil
	}
	//比较
	if (minTargetCost + cost1) >= (cost2 + minCurrentCost) {
		return &bestCurrentRotatePoint, nil
	}

	return &bestTargetRotatePoint, nil
}

func (tm *TrafficManager) GetRobotLocalPathFromDB(ctx context.Context, robotId string) (*model.LocalPath, codes.Code) {
	res := new(model.LocalPath)
	dbRes := tm.db.Collection(ctx, model.LocalPath{}.TableName()).FindOne(ctx, bson.M{"sn": robotId})
	if dbRes.Err() != nil {
		log.ErrorfWithContext(ctx, "get local path error:%s", dbRes.Err().Error())
		return nil, codes.ErrDatabase.Wrap(dbRes.Err())
	}
	if errBind := dbRes.Decode(res); errBind != nil {
		log.ErrorfWithContext(ctx, "bind local path error:%s", errBind.Error())
		return nil, codes.ErrDatabase.Wrap(errBind)
	}
	return res, nil
}

//************区域切换****

func (tm *TrafficManager) GetCrossPath(ctx context.Context, robot *model.Robot, mapName, target string, elevators map[string]*model.Elevator, robots []*model.Robot) ([]model.TaskInfo, float64, codes.Code) {
	newMapRelations, _ := tm.GetAllMapRelationFromDB(context.TODO())
	inOldCache := func(newMapRelation model.MapRelation, curMapRelations []model.MapRelation) bool {
		for _, cur := range curMapRelations {
			if newMapRelation.FromMap == cur.FromMap && newMapRelation.StartId == cur.StartId &&
				newMapRelation.ToMap == cur.ToMap && newMapRelation.GoalId == cur.GoalId &&
				newMapRelation.StartPathId == cur.StartPathId && newMapRelation.GoalPathId == cur.GoalPathId &&
				newMapRelation.CostValue == cur.CostValue && newMapRelation.IsTwoWay == cur.IsTwoWay &&
				newMapRelation.Type == cur.Type && newMapRelation.AgvId == cur.AgvId && newMapRelation.PositiveWay == cur.PositiveWay {
				return true
			}
		}
		return false
	}
	mapRelationDiff := func(newMapRelations, curMapRelations []model.MapRelation) bool {
		if len(curMapRelations) != len(newMapRelations) {
			return true
		}
		for _, newMapRelation := range newMapRelations {
			if !inOldCache(newMapRelation, curMapRelations) {
				return true
			}
		}
		return false
	}
	if mapRelationDiff(newMapRelations, tm.crossPlaner.mapRelations) {
		tm.crossPlaner.mapRelations, _ = tm.GetAllMapRelationFromDB(context.TODO())
		maps, _ := tm.GetAllMapFromDB(context.TODO())
		tm.crossPlaner.initDijkstraMap(maps)
	}
	sites, _ := tm.GetAllLockSiteFromDB(context.Background())
	for _, r := range robots {
		if r.Account.AgvId == robot.Account.AgvId {
			continue
		}
		if r.StatusIsExists() && tm.MapNameExists(r.Status.MapName) {
			node := tm.GetNode(r.Status.MapName, r.Status.SiteIdS)
			if node.Type == model.NodeElevator || node.Type == model.NodeSwitch {
				site := model.LockedSite{MapName: r.Status.MapName, SiteId: r.Status.SiteIdS}
				sites = append(sites, site)
			} else {
				for _, cover := range r.AgvCorverNodes {
					if cover == "" {
						continue
					}
					coverNode := tm.GetNode(r.Status.MapName, cover)
					if coverNode.Type == model.NodeElevator || coverNode.Type == model.NodeSwitch {
						site := model.LockedSite{MapName: r.Status.MapName, SiteId: coverNode.Id}
						sites = append(sites, site)
					}
				}
			}
		}
	}

	return tm.crossPlaner.getCrossPath(ctx, robot, mapName, target, elevators, sites)
}

// GetMapAndRelationDeeply 递归获取地图相关地图，返回包含自己的所有地图名
func (m *TrafficManager) GetMapAndRelationDeeply(ctx context.Context, mapName string) ([]string, codes.Code) {
	relationMaps := []string{}
	fromMaps := []string{mapName}
	for i := 1; i < 99; i++ {
		fromRelations, errFrom := m.GetMapRelationByFromMapsFromDB(ctx, fromMaps)
		if errFrom != nil {
			return nil, codes.ErrDatabase.Wrap(errFrom)
		}

		toRelations, err := m.GetMapRelationByToMapsFromDB(ctx, fromMaps)
		if err != nil {
			return nil, codes.ErrDatabase.Wrap(err)
		}
		relationMaps = append(relationMaps, fromMaps...)

		mapRelations := make([]*model.MapRelation, 0)
		if fromRelations == nil || len(fromRelations) < 1 {
			mapRelations = toRelations
		} else if toRelations == nil || len(toRelations) < 1 {
			mapRelations = fromRelations
		} else {
			mapRelations = append(fromRelations, toRelations...)
		}

		if mapRelations == nil || len(mapRelations) < 1 {
			return relationMaps, nil
		}

		newRelationMapNames := []string{}
		for _, m := range mapRelations {
			if !tools.StringSliceIsContain(relationMaps, m.ToMap) &&
				!tools.StringSliceIsContain(newRelationMapNames, m.ToMap) {
				newRelationMapNames = append(newRelationMapNames, m.ToMap)
			}

			if m.IsTwoWay && !tools.StringSliceIsContain(relationMaps, m.FromMap) &&
				!tools.StringSliceIsContain(newRelationMapNames, m.FromMap) {
				newRelationMapNames = append(newRelationMapNames, m.FromMap)
			}
		}

		if newRelationMapNames == nil || len(newRelationMapNames) < 1 {
			return relationMaps, nil
		}

		fromMaps = newRelationMapNames
	}
	return nil, codes.ErrDatabase
}

//1.设置地图切换信息
func (m *TrafficManager) AddMapRelationToDB(ctx context.Context, robots []*model.Robot, fromMap string, startId string, toMap string, goalId string, mapRelation *model.MapRelation) codes.Code {
	/*inMapList := func(mapName string, mapList []string) bool {
		for _, robot_map := range mapList {
			if mapName == robot_map {
				return true
			}
		}
		return false
	}*/

	minLength := 0.0
	if mapRelation.Type == model.FactoryRealtion {
		for _, r := range robots {
			//	if !inMapList(fromMap, r.Account.MapList) || !inMapList(toMap, r.Account.MapList) {
			//		continue
			//	}
			if r.Type.CategoryType == model.CategoryTypeBalanced || r.Type.CategoryType == model.CategoryTypeThreeWay || r.Type.GetRobotRotateRadius(r) > 2000 {
				continue
			}
			dis := 2 * (r.Type.GetRobotRotateRadius(r))
			if dis > minLength {
				minLength = dis
			}
		}
	} else {
		map_info1 := m.GetMapFF(fromMap)
		map_info2 := m.GetMapFF(toMap)
		floor1 := strings.Split(map_info1.ProjectId, "-")
		floor2 := strings.Split(map_info2.ProjectId, "-")
		if floor1[1] == floor2[1] {
			return codes.ErrSameFloor
		}
	}

	if fromMap == toMap {
		return codes.ErrRelationSameMap
	}
	planer, err := m.getPlaner(fromMap)
	if err != nil {
		log.Errorf("from_map 不存在")
		return err
	}
	from_paths := planer.getConnectPaths(planer.GetNode(startId))
	if len(from_paths) != 1 && len(from_paths) != 2 {
		log.Warnf("from_map start_id 绘制不合理")
		return codes.ErrMapNodeIllegal
	} else if from_paths[0].PathLength < float64(minLength) {
		return codes.ErrSwitchLength
	}
	planer, err = m.getPlaner(toMap)
	if err != nil {
		log.Errorf("to_map 不存在")
		return err
	}
	to_paths := planer.getConnectPaths(planer.GetNode(goalId))
	if len(to_paths) != 1 && len(to_paths) != 2 {
		log.Warnf("to_map goal_id 绘制不合理")
		return codes.ErrMapNodeIllegal
	} else if to_paths[0].PathLength < float64(minLength) {
		return codes.ErrSwitchLength
	}
	mapRelation.StartPathId = from_paths[0].PathId
	mapRelation.GoalPathId = to_paths[0].PathId

	dbRes := m.db.Collection(ctx, model.MapRelation{}.TableName()).FindOneAndReplace(ctx, bson.M{"from_map": fromMap, "start_id": startId, "to_map": toMap, "goal_id": goalId}, mapRelation, &options.FindOneAndReplaceOptions{Upsert: function.BoolPtr(true)})
	if dbRes.Err() != nil && !errors.Is(dbRes.Err(), mongo.ErrNoDocuments) {
		log.ErrorfWithContext(ctx, "insert and update map-relation  to db error:%s", dbRes.Err())
		return codes.ErrDatabase.Wrap(dbRes.Err())
	}
	return nil
}

//删除地图切换信息
func (m *TrafficManager) DeleteMapRelationByFromMapAndToMap(ctx context.Context, fomMap string, startId string, toMap string, goalId string) codes.Code {
	_, err := m.db.Collection(ctx, model.MapRelation{}.TableName()).DeleteOne(ctx, bson.M{"from_map": fomMap, "start_id": startId, "to_map": toMap, "goal_id": goalId})
	if err != nil && !errors.Is(err, mongo.ErrNoDocuments) {
		log.ErrorfWithContext(ctx, "delete map-relation from db error:s%s", err.Error())
		return codes.ErrDatabase.Wrap(err)
	}
	return nil
}
func (tm *TrafficManager) DeleteMapRelationByMapName(ctx context.Context, mapName string) codes.Code {
	_, errUpdate := tm.db.Collection(ctx, model.MapRelation{}.TableName()).DeleteMany(ctx, bson.M{"from_map": mapName})
	if errUpdate != nil {
		log.ErrorfWithContext(ctx, "delete map standby sites from db error:%s", errUpdate.Error())
		return codes.ErrDatabase.Wrap(errUpdate)
	}
	_, errUpdate = tm.db.Collection(ctx, model.MapRelation{}.TableName()).DeleteMany(ctx, bson.M{"to_map": mapName})
	if errUpdate != nil {
		log.ErrorfWithContext(ctx, "delete map standby sites from db error:%s", errUpdate.Error())
		return codes.ErrDatabase.Wrap(errUpdate)
	}
	return nil
}

// DeleteOverviewMapByName
func (tm *TrafficManager) DeleteOverviewMapByName(ctx context.Context, name string) codes.Code {
	_, errUpdate := tm.db.Collection(ctx, model.MapOverview{}.TableName()).DeleteMany(ctx, bson.M{"name": name})
	if errUpdate != nil {
		log.ErrorfWithContext(ctx, "delete overview map by name from db error:%s", errUpdate.Error())
		return codes.ErrDatabase.Wrap(errUpdate)
	}

	return nil
}

//获取地图切换信息
func (m *TrafficManager) GetAllMapRelationFromDB(ctx context.Context) ([]model.MapRelation, codes.Code) {
	mapRelationArr := make([]model.MapRelation, 0)
	res, err := m.db.Collection(ctx, model.MapRelation{}.TableName()).Find(ctx, bson.M{})
	if err != nil {
		log.ErrorfWithContext(ctx, "get all map-relation from db error:%s", err.Error())
		return nil, codes.ErrDatabase.Wrap(err)
	}
	if errBind := res.All(ctx, &mapRelationArr); errBind != nil {
		log.ErrorfWithContext(ctx, "bind all map-relation error:%s", errBind.Error())
		return nil, codes.ErrDatabase.Wrap(errBind)
	}
	return mapRelationArr, nil
}

//根据类型 查询厂区关联数据和楼层关联数据
func (m *TrafficManager) GetAllMapRelationByTypeFromDB(ctx context.Context, typeValue model.MapRelationType) ([]model.MapRelation, codes.Code) {
	mapRelationArr := make([]model.MapRelation, 0)
	res, err := m.db.Collection(ctx, model.MapRelation{}.TableName()).Find(ctx, bson.M{"type": typeValue})
	if err != nil {
		log.ErrorfWithContext(ctx, "get all map-relation from db error:%s", err.Error())
		return nil, codes.ErrDatabase.Wrap(err)
	}
	if errBind := res.All(ctx, &mapRelationArr); errBind != nil {
		log.ErrorfWithContext(ctx, "bind all map-relation error:%s", errBind.Error())
		return nil, codes.ErrDatabase.Wrap(errBind)
	}
	return mapRelationArr, nil
}

//获取地图关联的占用AGV
func (m *TrafficManager) GetMapRelationAGVFromDB(ctx context.Context, mapName, siteId string) string {
	dbRes := m.db.Collection(ctx, model.MapRelation{}.TableName()).FindOne(ctx, bson.M{"from_map": mapName, "start_id": siteId})
	if dbRes.Err() != nil {
		dbRes = m.db.Collection(ctx, model.MapRelation{}.TableName()).FindOne(ctx, bson.M{"to_map": mapName, "goal_id": siteId})
		if dbRes.Err() != nil {
			return ""
		}
	}
	res := new(model.MapRelation)
	if errBind := dbRes.Decode(res); errBind != nil {
		log.ErrorfWithContext(ctx, "bind robot local path error:%s", errBind.Error())
		return ""
	}
	return res.AgvId
}

// GetMapRelationByFromMapFromDB 通过fromMap 查找地图关联
func (m *TrafficManager) GetMapRelationByFromMapsFromDB(ctx context.Context, mapNames []string) ([]*model.MapRelation, codes.Code) {
	mapRelationArr := make([]*model.MapRelation, 0)
	res, err := m.db.Collection(ctx, model.MapRelation{}.TableName()).Find(ctx, bson.M{"from_map": bson.M{"$in": mapNames}})
	if err != nil {
		log.ErrorfWithContext(ctx, "get all map-relation from  by from maps error:%s", err.Error())
		return nil, codes.ErrDatabase.Wrap(err)
	}
	if errBind := res.All(ctx, &mapRelationArr); errBind != nil {
		log.ErrorfWithContext(ctx, "bind all map-relation error:%s", errBind.Error())
		return nil, codes.ErrDatabase.Wrap(errBind)
	}
	return mapRelationArr, nil
}

// GetMapRelationByToMapsFromDB 通过toMap 查找地图关联
func (m *TrafficManager) GetMapRelationByToMapsFromDB(ctx context.Context, mapNames []string) ([]*model.MapRelation, codes.Code) {
	mapRelationArr := make([]*model.MapRelation, 0)
	res, err := m.db.Collection(ctx, model.MapRelation{}.TableName()).Find(ctx, bson.M{"to_map": bson.M{"$in": mapNames}})
	if err != nil {
		log.ErrorfWithContext(ctx, "get all map-relation from  by from maps error:%s", err.Error())
		return nil, codes.ErrDatabase.Wrap(err)
	}
	if errBind := res.All(ctx, &mapRelationArr); errBind != nil {
		log.ErrorfWithContext(ctx, "bind all map-relation error:%s", errBind.Error())
		return nil, codes.ErrDatabase.Wrap(errBind)
	}
	return mapRelationArr, nil
}
func (tm *TrafficManager) CrossMapCanReach(startMap, endMap string) bool {
	return tm.crossPlaner.CrossMapCanReach(startMap, endMap)
}
